//*********************************************************************
// Copyright (C) 2014 Hell Prototypes / www.hellprototypes.com
// 
// This program is free software; you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation; either version 2 of the License, or (at
// your option) any later version.
//
// This program is distributed in the hope that it will be useful, but
// WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Public License for more details.
//
//********************************************************************
#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/power.h>
#include <util/atomic.h>
#include <avr/sleep.h>
#include "common.h"


static volatile uint16_t milliseconds;

#define MILLIS_TIMER_EN()		(TCC1.CTRLA = 0x04)

// Initialise library
void millis_init(void)
{
	//Chang system clock to ext 16M clock

	DFLLRC32M.CTRL = 0;

	CCP = CCP_IOREG_gc; //Security Signature to modify clock 
	CLK.CTRL = CLK_SCLKSEL_RC2M_gc; // Run at 2M

	OSC.CTRL = OSC_RC2MEN_bm;

	OSC.XOSCCTRL = 0xCB;	// Crystal type 0.4-16 MHz XTAL - 16K CLK Start Up time
	OSC.CTRL = OSC_RC2MEN_bm | OSC_XOSCEN_bm;

	while(!testbit(OSC.STATUS,OSC_XOSCRDY_bp));//wait

	CCP = CCP_IOREG_gc; //Security Signature to modify clock 
	CLK.CTRL = CLK_SCLKSEL_XOSC_gc; // Use xosc as system clock

	OSC.CTRL =  OSC_XOSCEN_bm;	 // Disable internal 2MHz

	//==============================================
	TCC1.PER = 1999;
	TCC1.INTCTRLA = 0x03; //HI Pri
	MILLIS_TIMER_EN();//DIV 8,  2M

	set_sleep_mode(SLEEP_MODE_IDLE);
	sei();
}

// Get current milliseconds
uint16_t millis_get()
{
	uint16_t ms;

	ATOMIC_BLOCK(ATOMIC_RESTORESTATE)
	{
		ms = milliseconds;
	}

	return ms;
}

ISR(TCC1_OVF_vect)
{
	milliseconds++;
}

void short_delay()
{
	ms_delay(10);
}

void ms_delay(uint16_t ms)
{
	uint16_t current, base, delay;

	base = millis_get();

	delay = ms;

	do {
		current = millis_get();

		if(current >= base) {
			if( current - base >= delay) {
				return;
			}
		} else {
			delay = delay - (0xFFFF - base);
			base = 0;
		}
		sleep_mode();
	} while(1);
}
